/*
Simple Program to activate LED1 and LED2 on the StorM32 board V1.32
it will generate a sinus pwm signal phase shifted on each winding for motor one
There is no control feedback
Optional a basich winding function can be tested (see in the main section
No additional libs needed, just copie into MBED online compiler main.cpp
Pinout refered to STorM32 BGC v130.sch
Have Fun :-)
*/
#include "mbed.h"
#define _M0_A0 PB_1
#define _M0_B0 PB_0
#define _M0_C0 PA_7

float pi = 3.1415;
#define ON 1
#define OFF 0
DigitalOut led_red(PB_13);
DigitalOut led_green(PB_12);

PwmOut  M0_A0(_M0_A0);
PwmOut  M0_B0(_M0_B0);
PwmOut  M0_C0(_M0_C0);

AnalogIn   ain1(PC_0); // joystick 1 
AnalogIn   ain2(PC_1);
AnalogIn   ain3(PC_2);

float delay_t =0.12;
float pwm_per = 0.0002;
float pwm_puls_perc = 0.15;

Serial pc(PC_10, PC_11);

int seq_steps[][3]= {
    {1,0,0},
    {1,1,0},

    {0,1,0},
    {0,1,1},

    {0,0,1},
    {1,0,1}
};

void seq(int a1,int a2,int a3,float pwm)
{
    M0_A0.write(a1 * pwm);
    M0_B0.write(a2 * pwm);
    M0_C0.write(a3 * pwm);
}

Ticker _ISR_1ms;
float dt_isr_sec = 0.0005;

float scale = 0.25; // 1 = 100% of PWM Signal
float offset = 0.5;
float amplitude = 0.5;
float phsA = 90,phsB = -30,phsC = -150;
float TphsA,TphsB,TphsC;
float angleA,angleB,angleC;
float Dyn_angle = 0,Dyn_angle_max = 360.0;

float joystick = 0;
float joys_midpos =0;
void ISR_1ms()
{
    led_green = !led_green;

    angleA = scale * (offset + amplitude * sin( TphsA+ 2*pi *(Dyn_angle/360)));
    angleB = scale * (offset + amplitude * sin( TphsB+ 2*pi *(Dyn_angle/360)));
    angleC = scale * (offset + amplitude * sin( TphsC+ 2*pi *(Dyn_angle/360)));

    M0_A0.write(angleA);
    M0_B0.write(angleB);
    M0_C0.write(angleC);

    joystick = (joys_midpos - ain1) * 5; // Replace mit any control algorithm eg IMU PID Controller --> use error of deg to calculate deviation

    Dyn_angle = Dyn_angle + joystick;
}

int main()
{
    joys_midpos = ain1;
    TphsA = 2.0 * pi /(360.0/phsA);
    TphsB = 2.0 * pi /(360.0/phsB);
    TphsC = 2.0 * pi /(360.0/phsC);

    _ISR_1ms.attach(&ISR_1ms, dt_isr_sec);

    M0_A0.period(pwm_per);
    M0_B0.period(pwm_per);
    M0_C0.period(pwm_per);

    wait(0.5);
    led_green = OFF;

    while(1) {
        int total = sizeof(seq_steps);
        int column = sizeof(seq_steps[0]);
        int row = total / column;
        delay_t= 0.005;
        pwm_puls_perc = 0.2;

        pc.printf("%.2f %.2f %.2f %.2f %.2f \n",angleA,angleB,angleC,joystick,Dyn_angle);
        // Use arduino IDE to view the sinus signals (Tools --> Serial Plotter

        for(int j=0; j<row; j++) {
            // Optional use block commutation
            //seq(seq_steps[j][0],seq_steps[j][1],seq_steps[j][2],pwm_puls_perc);
            //wait(delay_t);
        }

    }
}